#!/usr/bin/env python3
# -*- encoding: utf-8 -*-
'''
@File    :   ap_mapping_data_check.py
@Time    :   2024/01/02 17:21:45
@Author  :   YcZ
@Version :   1.0
@Contact :   yuchao.zhou@outlook.com
@License :   (C)Copyright 2022-2024, YcZ
@Desc    :   
'''


import time

from cyber.python.cyber_py3 import record
from modules.common_msgs.sensor_msgs.gnss_best_pose_pb2 import GnssBestPose
from modules.common_msgs.sensor_msgs.imu_pb2 import Imu
from modules.common_msgs.sensor_msgs.pointcloud_pb2 import PointCloud


apollo_bestpos_type = "apollo.drivers.gnss.GnssBestPose"
apollo_imu_type = 'apollo.drivers.gnss.Imu'
apollo_pc_type = 'apollo.drivers.PointCloud'

imu_gap_threshold = 0.1
pc_gap_threshold = 0.3
std_gap_threshold = 0.06


def printgnss(gnss_msg):
    print("gnss msg time: ", gnss_msg.header.timestamp_sec)
    print("measurement_time: ", gnss_msg.measurement_time)
    print("sol_status:", gnss_msg.sol_status)
    print("sol_type:", gnss_msg.sol_type)
    print("latitude:", gnss_msg.latitude)
    print("longitude:", gnss_msg.longitude)
    print("height_msl:", gnss_msg.height_msl)
    print("undulation:", gnss_msg.undulation)
    print("datum_id:", gnss_msg.datum_id)
    print("latitude_std_dev:", gnss_msg.latitude_std_dev)
    print("longitude_std_dev:", gnss_msg.longitude_std_dev)
    print("height_std_dev:", gnss_msg.height_std_dev)
    print("num_sats_tracked:", gnss_msg.num_sats_tracked)
    print("num_sats_in_solution:", gnss_msg.num_sats_in_solution)

'''
header:
  timestamp_sec: 1697701497.134002686
  module_name: gnss
  sequence_num: 1537
measurement_time: 1381736715.000000000
sol_status: SOL_COMPUTED
sol_type: NARROW_INT
latitude: 31.290819551
longitude: 121.208535599
height_msl: 4.438874098
undulation: 10.100000
datum_id: WGS84
latitude_std_dev: 0.016176
longitude_std_dev: 0.020298
height_std_dev: 0.050468
base_station_id: 1108
differential_age: 2.000000
solution_age: 0.000000
num_sats_tracked: 31
num_sats_in_solution: 31
num_sats_l1: 31
num_sats_multi: 19
extended_solution_status: 33
galileo_beidou_used_mask: 48
gps_glonass_used_mask: 51
'''


def apollo_record_reader(reader_path):
    """
    Record reader.
    """
    freader = record.RecordReader(reader_path)
    time.sleep(1)
    print('=' *100)
    print(f'+ Begin to read record file: {reader_path} +')
    imu_count = 0
    pc_count = 0
    gnss_count = 0
    gnss_std_count = 0
    lastimumeasuretime = 0
    lastpcmeasuretime = 0
    print('=' *100)
    lat_max = 0 
    lat_min = 0 
    lon_max = 0 
    lon_min = 0 
    for channel_name, msg, datatype, timestamp in freader.read_messages():
        if datatype == apollo_imu_type:
            msg_new = Imu()
            msg_new.ParseFromString(msg)
            if lastimumeasuretime < 1:
                pass
            elif msg_new.measurement_time - lastimumeasuretime > imu_gap_threshold: 
                imu_count += 1
                print("measurement_time", msg_new.measurement_time)
                print("last measurement_time              ", lastimumeasuretime)
                print("imu gap", msg_new.measurement_time - lastimumeasuretime)
                print("-"*100)
            else:
                pass
            lastimumeasuretime = msg_new.measurement_time
        elif datatype == apollo_pc_type:
            msg_new = PointCloud()
            msg_new.ParseFromString(msg)
            if lastpcmeasuretime < 1:
                pass
            elif msg_new.measurement_time - lastpcmeasuretime > pc_gap_threshold: 
                pc_count += 1
                print("measurement_time", msg_new.measurement_time)
                print("last measurement_time              ", lastpcmeasuretime)
                print("pointcloud gap", msg_new.measurement_time - lastpcmeasuretime)
                print("-"*100)
            else:
                pass
            lastpcmeasuretime = msg_new.measurement_time
        
        elif datatype == apollo_bestpos_type:
            msg_new = GnssBestPose()
            msg_new.ParseFromString(msg)            
            if lat_max == 0:
                lat_max = msg_new.latitude
            if lat_min == 0:
                lat_min = msg_new.latitude
            if lon_max == 0:
                lon_max = msg_new.longitude
            if lon_min == 0:
                lon_min = msg_new.longitude

            lat_max = max(lat_max, msg_new.latitude)
            lon_max = max(lon_max, msg_new.longitude)
            lat_min = min(lat_min, msg_new.latitude)
            lon_min = min(lon_min, msg_new.longitude)

            # if msg_new.sol_type is not "NARROW_INT":
            if msg_new.sol_type is not 50:
                gnss_count += 1
                print("gnss not Narrow Int at ", timestamp)
                printgnss(msg_new)
                print("-"*100)
            
            if msg_new.latitude_std_dev > std_gap_threshold or msg_new.longitude_std_dev > std_gap_threshold: 
                gnss_std_count += 1
                print(f"gnss lat or lon std over {std_gap_threshold} at {timestamp}")
                printgnss(msg_new)
                print("-"*100)

    print(f"imu time gap over {imu_gap_threshold}s count:", imu_count)
    print(f"pointcloud gap over {pc_gap_threshold}s count:", pc_count)
    print("gnss not Narrow Int count:", gnss_count)
    print(f"gnss lat or lon std over {std_gap_threshold} count:", gnss_std_count)
    print(f"latitude range: [{lat_min}, {lat_max}]")
    print(f"longitude range: [{lon_min}, {lon_max}]")
    


if __name__ == '__main__':
    import sys
    # print(len(sys.argv))

    test_record_file = ""
    if len(sys.argv) == 2:
        test_record_file = sys.argv[1]
    else:
        raise Exception("Require file name or folder name")
    import os
    if os.path.isfile(test_record_file):
        apollo_record_reader(test_record_file)
    elif os.path.isdir(test_record_file):
        for ff in sorted(os.listdir(test_record_file)):
            apollo_record_reader(os.path.join(test_record_file,ff))
    else:
        raise Exception("File or folder not exist")
    